工具动力学/重力参数在线辨识接口说明

目前基于电流的负载参数在线辨识只有一种类型：type = TOOL_GRAVITY_JNT_CURRENT_ONLINE(基于关节电流的工具重力参数在线辨识)，其基本流程和接口如下：

1.设置相关参数

1.1 建立机器人算法接口实例

/**
* @brief 初始化算法库, 生成机器人模型
* @param urdfModel: urdf模型, 以字符串形式描述
* @param srdfModel: srdf模型, 以字符串形式描述
* @return < 0, 表示初始化失败
*/
ARAL_API_BASIC(1.0) int rlInitiateRobotModel(const std::string& urdfModel, const std::string& srdfModel) = 0;

/**
* @brief 初始化算法库, 设置机械臂模型及配置参数信息(算法会自动加载对应的文件)
* @param robot_name: 机械臂名称
* @param path: urdf, meshes等资源文件夹所在的路径(如./aral_export/aubo_description/)
*/
ARAL_API_BASIC(1.0) int rlInitiateRobotModelFromFiles(const std::string& robot_name, const std::string& path) = 0;

1.2 设置力矩常数

/**
* @brief 设置机械臂各关节的力矩常数, 单位: (Nm/A)
* @param moduleConstants: 力矩常数, 向量的维度应该和关节个数一致
*/
ARAL_API_COMMON(1.0) void mdlSetJointTorqueConstant(const RLJntArray& moduleConstants) = 0;

1.3设置摩擦力参数

 /**
 * @brief 设置关节摩擦参数, 该参数一般通过读取关节驱动中的参数而获得
 * @param friPara: 摩擦模型参数结构体, 长度为机器人自由度(一般是6), 顺序为[J1,J2,J3,J4,J5,J6]
 *                 类型为 VTL_Model 速度-温度-载荷模型
 *                 具体参数及顺序为[Fs, Fc, Vs, Miu, Fv0, Fv1, Fv2, Fv3, Ft1, Ft2, Ft3, c1, c2]
 * @return < 0, 表示设置失败
 */
ARAL_API_COMMON(1.0) int mdlSetJointFrictionParameter(const std::vector<FrictionParam>& friPara) = 0;

1.4 设置重力加速度

/**
 * @brief 设置重力加速度向量在机械臂基坐标系下的描述
 * @param vec[0]: 重力加速度在机械臂基坐标系X轴上的分量
 *        vec[1]: 重力加速度在机械臂基坐标系Y轴上的分量
 *        vec[2]: 重力加速度在机械臂基坐标系Z轴上的分量
 * @return if < 0, 表示设置失败
 */
ARAL_API_COMMON(1.0) int mdlSetGravityVectorInBase(const Array3d& vec) = 0;

1.5设置动力学参数

/**
  * @brief 设置机械臂的连杆动力学参数
  * @param real_para: 实际辨识出来的参数，参数格式为: real_para = [参数类型, 辨识的参数]
  *                   1. real_para[0] = 0; size(real_para) = 60 + 1, 输入参数含义为 m, mx, my, mz, ixx, ixy, ixz, iyy, iyz, izz ..., 需要另外设置转子惯量．否则为urdf里面的值．
  *                   2. real_para[0] = 1; size(real_para) = 58 + 1, 对应 aral 动力学标定接口的数据输出格式.
  *                   3. real_para[0] = 2; size(real_para) = 42 + 1, 对应第一版拖动示教导入生产写在底座的数据格式．
  * @return: if < 0, 则设置的参数类型或大小不正确
  */
ARAL_API_COMMON(1.0) int mdlSetRobotLinkDynamicParameter(const std::vector<double>& real_para) = 0;

1.6 设置激励轨迹和辨识配置信息

struct CalibConfig
{
    DynCalibType        type;                                               // 动力学参数辨识类型
    int                 sample_frequency{ARAL_TRAJECTORY_SAMPLE_FREQUENCY}; // 测量轨迹的采样频率
};//工具标定配置信息

其中type = TOOL_GRAVITY_JNT_CURRENT_ONLINE = 100, //基于关节电流的工具重力参数在线辨识 sample_frequency为采样频率(默认值为200)

2. 运行轨迹

在线辨识即指机器人正常运动的用户点位

3.采集数据

3.1 采集数据

基于关节电流的工具重力参数在线辨识：带载运行激励轨迹，记录以下数据：关节角q，速度qd，加速度qdd，关节电流current，温度temp，末端力传感器F_end，底座力传感器F_base

 *数据格式如下：
 *q1 ... q6, qd1...qd6,...，F_base1 ... F_base6 带载数据，共n行

软件部完成此步骤

3.2 加载采集的数据

软件部完成此步骤

4.负载辨识

4.1 调用如下接口，实现在线辨识负载功能

    /**
     * @brief 负载动力学参数在线辨识(工具辨识)
     * @param trajFeature: 辨识需设置的参考轨迹特征信息(包括辨识类型, 运动属性和激励轨迹特征系数)
     *                     trajFeature.config.: 为激励轨迹的配置参数,包含辨识类型及运动属性
     *                       trajFeature.config.type = TOOL_GRAVITY_JNT_CURRENT_ONLINE;
     *                     trajFeature.config.sample_frequency = 200(实际采样频率，一般默认为200);
     *                     trajFeature.coeff: 为激励轨迹的特征系数,目前仅傅利叶级数激励激励轨迹具有该系数
     * @param is_first: 是否第一次调用该函数
     * @param tool_para: 初始负载参数(额定负载参数/上一次辨识获得的负载参数)
     * @param data_raw: 用户传入的测量参数(参数传入顺序与http://git.aubo-robotics.cn:8001/aral/aral_export/-/issues/541里面规定的一致)
     * @param results: 每个时刻输出的估计参数， 具体的数值意义根据不同的type有不同的定义
     *        if type = TOOL_GRAVITY_JNT_CURRENT_ONLINE;
     *        results[i].linkpara = [m,cx,cy,cz,ixx,iyy,izz,ixy,ixz,iyz]; 第i个时刻下辨识得到的负载参数[质量，在法兰坐标系下的质心，在法兰坐标系下的惯性张量]
     *        results[i].error = [average,max,rms]double {0.0};              // 第i个时刻下关节力矩误差
     * @return: 返回值 < 0 表示计算失败
     */
    ARAL_API_COMMON(1.0)  int calibDynamicParaOnline(const CalibTrajectoryFeature& trajFeature, bool is_first, const RLInertia& tool_para, const DoubleVecVec& data_raw, std::vector<ToolCalibResult>& results) = 0;

• is_first为true时，即第一次调用该函数，负载初始值如果已知，正常设置，如果未知，建议质量设置为额定负载，其余参数设置为0
• 传入数据data_raw时，为了提高辨识结果准确度，数据量应不少于400

5.调用示例

TEST_FIXTURE(AuboRobotInterface, testToolDynamicIdentifyOnline)
{
    //! step1: 设置相关参数
    //! step1.1: 设置机器人类型
    Setup("aubo_i5");
    //! step1.1: 设置力矩系数
    robot->mdlSetJointTorqueConstant(motor_const);
    //! step1.3: 设置摩擦参数
    robot->mdlSetJointFrictionParameter(friction_para);
    //! step1.4: 设置重力加速度
    robot->mdlSetGravityVectorInBase(gravity_acc);
    //! step1.5: 设置动力学参数
    robot->mdlSetRobotLinkDynamicParameter(link_para);
    //! step1.6: 设置激励轨迹和辨识配置参数
    interface::CalibConfig excite_traj_config;
    excite_traj_config.type         = 100;
    excite_traj_config.sample_frequency = 200;//采样频率
    std::vector<double> traj_parameter;
    trajFeature.coeff = traj_parameter;
    trajFeature.config = excite_traj_config;

    //! step2: 负载辨识
    std::vector<interface::ToolCalibResult> paraEst0, paraEst1, paraEst2;
    MatrixNd tool_para_0(10, 1);  // 创建一个 10x1 的矩阵
    tool_para_0 << 10, 0, 0, 0, 0, 0, 0, 0, 0, 0;  // 负载辨识初始值，质量可以用额定负载，其余参数为0 
    unsigned int total_rows = data_raw.size();
    unsigned int group_size = total_rows / 3; // 每组的行数
    // 计算余数
    unsigned int remainder = total_rows % 3;

    // 将数据均分为三组
    for (unsigned int i = 0; i < group_size; ++i)
    {
        data_raw_0.push_back(data_raw[i]);
        data_raw_1.push_back(data_raw[i + group_size]);
        data_raw_2.push_back(data_raw[i + 2 * group_size]);
    }

    // 处理余下的行
    for (unsigned int i = 0; i < remainder; ++i)
    {
        data_raw_2.push_back(data_raw[total_rows - remainder + i]);
    }
    //第一次调用，tool_para_0用额定负载
    ret = robot->calibDynamicParaOnline(trajFeature, true, tool_para_0, data_raw_0, paraEst0);
    tool_para_1 = paraEst0.back().inertia;
    //第二次调用，tool_para_1用上一次辨识计算得到的值
    ret = robot->calibDynamicParaOnline(trajFeature, false, tool_para_1, data_raw_1, paraEst1);
    tool_para_2 = paraEst1.back().inertia;
    //第三次调用，tool_para_2用上一次辨识计算得到的值
    ret = robot->calibDynamicParaOnline(trajFeature, false, tool_para_2, data_raw_2, paraEst2);
}